// BLDC driver standalone example
#include <SimpleFOC.h>

#define LED_PIN 11

// angle set point variable
float target = 1;

// init BLDC motor
BLDCMotor motor = BLDCMotor( 11 );

// BLDC driver instance
BLDCDriver3PWM driver = BLDCDriver3PWM(3, 5, 6, 7);

// MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb)
//  chip_address         - I2C chip address
//  bit_resolution       - resolution of the sensor
//  angle_register_msb   - angle read register msb
//  bits_used_msb        - number of used bits in msb register
MagneticSensorI2C encoder  = MagneticSensorI2C(0x36, 12, 0x0E, 4);
// or quick config
//MagneticSensorI2C encoder  = MagneticSensorI2C(AS5600_I2C);

void setup() {

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  _delay(2000);
  // monitoring port
  Serial.begin(115200);

  // initialize encoder hardware
  encoder.init();
  // hardware interrupt enable
  // encoder.enableInterrupts(doA, doB);
  // link the motor to the sensor
  motor.linkSensor(&encoder);

  // power supply voltage
  // default 12V
  driver.voltage_power_supply = 12;
  driver.init();
  // link the motor to the driver
  motor.linkDriver(&driver);

  // set control loop to be used
  motor.controller = MotionControlType::angle;

  // controller configuration based on the control type
  // velocity PI controller parameters
  // default P=0.5 I = 10
  motor.PID_velocity.P = 0.2;
  motor.PID_velocity.I = 20;
  // jerk control using voltage voltage ramp
  // default value is 300 volts per sec  ~ 0.3V per millisecond
  motor.PID_velocity.output_ramp = 1000;

  //default voltage_power_supply
  motor.voltage_limit = 6;

  // velocity low pass filtering
  // default 5ms - try different values to see what is the best.
  // the lower the less filtered
  motor.LPF_velocity.Tf = 0.01;

  // angle P controller
  // default P=20
  motor.P_angle.P = 20;
  //  maximal velocity of the position control
  // default 20
  motor.velocity_limit = 4;

  // initialize motor
  motor.init();
  // align encoder and start FOC
  motor.initFOC();

  // add target command T
  // command.add('T', doTarget, "target angle");

  Serial.println("Motor ready.");
  Serial.println("Set the target angle using serial terminal:");
}

void loop() {
  // iterative FOC function
 // motor.loopFOC();
  digitalWrite(LED_PIN, HIGH);
    _delay(500);
  // function calculating the outer position loop and setting the target position
  //motor.move(target);
  digitalWrite(LED_PIN, LOW);
    _delay(500);
}
